

#include <iostream>
#include <iomanip>      // std::setprecision
#include <string>
#include <armadillo>
#include <vector>

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "boost/algorithm/string.hpp"
#include "google/protobuf/text_format.h"

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/net.hpp"
#include "caffe/proto/caffe.pb.h"
#include "caffe/util/db.hpp"
#include "caffe/util/io.hpp"
#include "caffe/vision_layers.hpp"


#define _USE_MATH_DEFINES

#include <cmath>

#include <sstream>
#include <cstdlib>
#include <chrono>

#define SENDTOCLIENT
//#define SAVETOTXT


#ifdef SENDTOCLIENT

#include "ToTK1.hpp"
#include "ToArduinoA.hpp"
#endif


using namespace cv;

template<typename Dtype>
void preprocess(const cv::Mat &image,
                const Dtype mean,
                cv::Mat &image_out) {

    cv::Mat temp;
    image.convertTo(temp, CV_32FC3);
    temp = temp.mul(1 / 256.0f);
    //temp = temp - mean;

    cv::add(temp, -0.5, temp);

    image_out = temp.clone();

    return;
}










// ======================== COPY FROM TX1 (9/F) ===================================
#define HISTMATCH_EPSILON 0.000001

// Compute histogram and CDF for an image with mask
void do1ChnHist(const Mat_<uchar> &img, const Mat_<uchar> &mask, Mat_<double> &h, Mat_<double> &cdf)
{
    for(size_t p=0; p<img.total(); p++)
    {
        if(mask(p) > 0)
        {
            uchar c = img(p);
            h(c) += 1.0;
        }
    }

    normalize(h,h,1,0,NORM_MINMAX);

    cdf(0) = h(0);
    for(int j=1; j<256; j++)
    {
        cdf(j) = cdf(j-1) + h(j);
    }

    normalize(cdf,cdf,1,0,NORM_MINMAX);
}

// match histograms of 'src' to that of 'dst', according to both masks
void histMatchRGB(Mat &src, const Mat &src_mask, const Mat &dst, const Mat &dst_mask)
{
    //#ifdef BTM_DEBUG
    //    namedWindow("original source",CV_WINDOW_AUTOSIZE);
    //    imshow("original source",src);
    //    namedWindow("original query",CV_WINDOW_AUTOSIZE);
    //    imshow("original query",dst);
    //#endif

    vector<Mat_<uchar> > chns, chns1;
    split(src,chns);
    split(dst,chns1);

    for (int i=0; i<3; i++)
    {
        Mat_<double> src_hist = Mat_<double>::zeros(1,256);
        Mat_<double> dst_hist = Mat_<double>::zeros(1,256);
        Mat_<double> src_cdf  = Mat_<double>::zeros(1,256);
        Mat_<double> dst_cdf  = Mat_<double>::zeros(1,256);

        do1ChnHist(chns[i],  src_mask, src_hist, src_cdf);
        do1ChnHist(chns1[i], dst_mask, dst_hist, dst_cdf);

        uchar last = 0;

        Mat_<uchar> lut(1,256);
        for (int j=0;j<src_cdf.cols;j++)
        {
            double F1j = src_cdf(j);

            for (uchar k = last; k<dst_cdf.cols; k++)
            {
                double F2k = dst_cdf(k);
                if(abs(F2k - F1j) < HISTMATCH_EPSILON || F2k > F1j)
                {
                    lut(j) = k;
                    last = k;
                    break;
                }
            }
        }

        LUT(chns[i],lut,chns[i]);
    }

    Mat res;
    merge(chns,res);

    //#ifdef BTM_DEBUG
    //   namedWindow("matched",CV_WINDOW_AUTOSIZE);
    //    imshow("matched",res);
    //
    //    waitKey(0);
    //#endif

    res.copyTo(src);
}
// =======================================================================















void thresh_callback(const cv::Mat &image, const int thresh, std::vector<cv::Rect> &bbox)
{
    cv::Mat threshold_output;
    vector<vector<Point> > contours;
    vector<Vec4i> hierarchy;
    RNG rng(12345);

    /// Detect edges using Threshold
    cv::Mat image_8u;
    image.convertTo(image_8u, CV_8UC1, 200, 0);
    for (int i = 0; i < image.rows; i++) {
        for (int j = 0; j < image.cols; j++) {
            //std::cout<<image.at<float>(i,j)<<":"<<image_8u.at<float>(i,j)<<", ";
        }
    }

    cv::threshold(image_8u, threshold_output, thresh, 255, THRESH_BINARY );
    //cv::imshow("image_8u", image_8u);
    //cv::imshow("image", image);
    //cv::imshow("thres", threshold_output);
    //cv::waitKey(0);

    /// Find contours
    findContours( threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );

    /// Approximate contours to polygons + get bounding rects and circles
    vector<vector<Point> > contours_poly( contours.size() );
    vector<Rect> boundRect( contours.size() );
    vector<Point2f>center( contours.size() );
    vector<float>radius( contours.size() );

    //std::cout<<"contour.size() = "<<contours.size()<<std::endl;
    for( int i = 0; i < contours.size(); i++ ) {
        approxPolyDP( Mat(contours[i]), contours_poly[i], 3, true );
        boundRect[i] = cv::boundingRect(Mat(contours_poly[i]));
        bbox.push_back(boundRect[i]);
        minEnclosingCircle( (Mat)contours_poly[i], center[i], radius[i] );
    }

    /// Draw polygonal contour + bonding rects + circles
    Mat drawing = Mat::zeros( threshold_output.size(), CV_8UC3 );
    for( int i = 0; i< contours.size(); i++ ) {
        Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
        drawContours( drawing, contours_poly, i, color, 1, 8, vector<Vec4i>(), 0, Point() );
        rectangle( drawing, boundRect[i].tl(), boundRect[i].br(), color, 2, 8, 0 );
        circle( drawing, center[i], (int)radius[i], color, 2, 8, 0 );
    }

    /// Show in a window
    //namedWindow( "Contours", CV_WINDOW_AUTOSIZE );
    //cv::imshow( "Contours", drawing );
    //cv::waitKey(0);
}


template<typename Dtype>
void linecross(const Dtype slope1,
               const cv::Point &p1,
               const Dtype slope2,
               const cv::Point &p2,
               cv::Point &cross_point) {

    // Compute the cross point given two lines
    // input - SlopeX, pointX: the slope of the line, and one point on the line
    // output - x: the [x, y] coordinate of the cross point
    //
    // Ax=b
    //
    cv::Mat A = (cv::Mat_<Dtype>(2, 2) << slope1, -1, slope2, -1);
    cv::Mat b = (cv::Mat_<Dtype>(2, 1) << slope1*p1.x - p1.y, slope2*p2.x - p2.y);
    cv::Mat x;

    cv::solve(A, b, x);


    cross_point = cv::Point(x.at<Dtype>(0,0), x.at<Dtype>(0,1));

}


template<typename Dtype>
void refine_joints(const std::vector<cv::Point> &joints,
                   const std::vector<Dtype> &joints_scores,
                   std::vector<cv::Point> &refined_joints,
                   std::vector<Dtype> &refined_joints_scores) {

    std::vector<cv::Point> vertical_mid;

#if 0
    cv::Mat point_23_x = cv::Mat::zeros(1, 2, CV_8UC1);
    cv::Mat point_23_y = cv::Mat::zeros(1, 2, CV_8UC1);
    cv::Mat point_45_x = cv::Mat::zeros(1, 2, CV_8UC1);
    cv::Mat point_45_y = cv::Mat::zeros(1, 2, CV_8UC1);


    for (int i = 0; i < joints.size(); i++) {
        std::cout<<"joints["<<i<<"] = "<<joints[i]<<std::endl;
        std::cout<<"joints["<<i<<"].x = "<<joints[i].x<<", joints["<<i<<"].y = "<<joints[i].y<<std::endl;
    }
    point_23_x.at<unsigned char>(0,0) = joints[1].x;
    point_23_x.at<unsigned char>(0,1) = joints[2].x;
    point_23_y.at<unsigned char>(0,0) = joints[1].y;
    point_23_y.at<unsigned char>(0,1) = joints[2].y;
    point_45_x.at<unsigned char>(0,0) = joints[3].x;
    point_45_x.at<unsigned char>(0,1) = joints[4].x;
    point_45_y.at<unsigned char>(0,0) = joints[3].y;
    point_45_y.at<unsigned char>(0,1) = joints[4].y;

    std::cout<<"point_23_x = "<<point_23_x<<", point_23_y = "<<point_23_y<<", point_45_x = "<<point_45_x<<std::endl;
    std::cout<<"cv::mean(point_23_x) = "<<(cv::mean(point_23_x))[0]<<std::endl;
    vertical_mid.push_back(cv::Point(cv::mean(point_23_x)[0], cv::mean(point_23_y)[0]));
    std::cout<<"point_23_x = "<<point_23_x<<", point_23_y = "<<point_23_y<<"vertical_mid[0] = "<<vertical_mid[0]<<std::endl;
    vertical_mid.push_back(cv::Point(cv::mean(point_45_x)[0], cv::mean(point_45_y)[0]));
#endif


    vertical_mid.push_back(cv::Point((joints[1].x+joints[2].x)/2, (joints[1].y+joints[2].y)/2));
    vertical_mid.push_back(cv::Point((joints[3].x+joints[4].x)/2, (joints[3].y+joints[4].y)/2));

    std::vector<cv::Point> horizontal_mid;

    horizontal_mid.push_back(cv::Point((joints[1].x+joints[4].x)/2, (joints[1].y+joints[4].y)/2));
    horizontal_mid.push_back(cv::Point((joints[2].x+joints[3].x)/2, (joints[2].y+joints[3].y)/2));

    cv::Mat score_23 = cv::Mat::zeros(1, 2, CV_32FC1);
    cv::Mat score_34 = cv::Mat::zeros(1, 2, CV_32FC1);
    cv::Mat score_45 = cv::Mat::zeros(1, 2, CV_32FC1);
    cv::Mat score_25 = cv::Mat::zeros(1, 2, CV_32FC1);

    score_23.at<Dtype>(0,0) = joints_scores[1];
    score_23.at<Dtype>(0,1) = joints_scores[2];

    score_34.at<Dtype>(0,0) = joints_scores[2];
    score_34.at<Dtype>(0,1) = joints_scores[3];

    score_45.at<Dtype>(0,0) = joints_scores[3];
    score_45.at<Dtype>(0,1) = joints_scores[4];

    score_25.at<Dtype>(0,0) = joints_scores[1];
    score_25.at<Dtype>(0,1) = joints_scores[4];

    std::vector<Dtype> scores;
    scores.push_back(cv::mean(score_23)[0]);
    scores.push_back(cv::mean(score_34)[0]);
    scores.push_back(cv::mean(score_45)[0]);
    scores.push_back(cv::mean(score_25)[0]);

    Dtype max = scores[0];
    int max_index = 0;
    for (int i = 1; i < scores.size(); i++) {
        if (scores[i] > max) {
            max = scores[i];
            max_index = i;
        }
    }

    refined_joints[0] = cv::Point(joints[0].x, joints[0].y);
    refined_joints[5] = cv::Point(joints[5].x, joints[5].y);

    // adjust the rest of edges according to the edge whose score is the highest.
    if (max_index == 0) {
        Dtype slope1 = (joints[1].y - joints[2].y) / (Dtype)(joints[1].x - joints[2].x);
        Dtype slope2 = -1/slope1;

        linecross(slope1, vertical_mid[1], slope2, horizontal_mid[0], refined_joints[4]);
        linecross(slope1, vertical_mid[1], slope2, horizontal_mid[1], refined_joints[3]);
        linecross(slope1, vertical_mid[0], slope2, horizontal_mid[0], refined_joints[1]);
        linecross(slope1, vertical_mid[0], slope2, horizontal_mid[1], refined_joints[2]);
    } else if (max_index == 1) {
        Dtype slope1 = (joints[2].y - joints[3].y) / (Dtype)(joints[2].x - joints[3].x);
        Dtype slope2 = -1/slope1;

        linecross(slope1, horizontal_mid[1], slope2, vertical_mid[0], refined_joints[2]);
        linecross(slope1, horizontal_mid[1], slope2, vertical_mid[1], refined_joints[3]);
        linecross(slope1, horizontal_mid[0], slope2, vertical_mid[0], refined_joints[1]);
        linecross(slope1, horizontal_mid[0], slope2, vertical_mid[1], refined_joints[4]);
    } else if (max_index == 2) {
        Dtype slope1 = (joints[3].y - joints[4].y) / (Dtype)(joints[3].x - joints[4].x);
        Dtype slope2 = -1/slope1;

        linecross(slope1, vertical_mid[1], slope2, horizontal_mid[0], refined_joints[4]);
        linecross(slope1, vertical_mid[1], slope2, horizontal_mid[1], refined_joints[3]);
        linecross(slope1, vertical_mid[0], slope2, horizontal_mid[0], refined_joints[1]);
        linecross(slope1, vertical_mid[0], slope2, horizontal_mid[1], refined_joints[2]);
    } else if (max_index == 3) {
        Dtype slope1 = (joints[1].y - joints[4].y) / (Dtype)(joints[1].x - joints[4].x);
        Dtype slope2 = -1/slope1;

        linecross(slope1, horizontal_mid[1], slope2, vertical_mid[0], refined_joints[2]);
        linecross(slope1, horizontal_mid[1], slope2, vertical_mid[1], refined_joints[3]);
        linecross(slope1, horizontal_mid[0], slope2, vertical_mid[0], refined_joints[1]);
        linecross(slope1, horizontal_mid[0], slope2, vertical_mid[1], refined_joints[4]);

    }
}



template<typename Dtype>
void object_score(const std::vector<cv::Point> &joints,
                  const std::vector<cv::Mat> &score_maps,
                  std::vector<Dtype> &joints_scores,
                  Dtype &average_score) {

    //std::cout<<"joints.size() = "<<joints.size()<<std::endl;

    int x_min = joints[0].x, x_max = joints[0].x;
    int y_min = joints[0].y, y_max = joints[0].y;
    for (int i = 1; i < joints.size(); i++) {
        if (joints[i].x > x_max) {
            x_max = joints[i].x;
        } else if (joints[i].x < x_min) {
            x_min = joints[i].x;
        } else if (joints[i].y > y_max) {
            y_max = joints[i].y;
        } else if (joints[i].y < y_min) {
            y_min = joints[i].y;
        }
    }

    if (x_min < 1 || y_min < 1 || x_max > score_maps[0].cols || y_max > score_maps[0].rows) {
        average_score = 0;
    } else {
        Dtype total_score = 0;
        for (int i = 0; i < joints.size(); i++) {
            joints_scores.push_back(score_maps[i].at<float>(joints[i].y, joints[i].x));
            total_score += joints_scores[i];
        }
        average_score = total_score/joints.size();
    }
}


template<typename Dtype>
void resizeIntoScaledImg(const std::vector<cv::Mat> &score_maps,
                         const std::vector<int> &pad,
                         std::vector<cv::Mat> &out_score_maps) {

    int np = score_maps.size() - 1;
    //std::cout<<"np = "<<np<<std::endl;


    int h = score_maps[0].rows;
    int w = score_maps[1].cols;

    CHECK_EQ(h, w)<<"h != w\n";

    //std::cout<<"h = "<<h<<", w = "<<w<<std::endl;
    cv::Point2f center((w / 2), (h / 2));

#if 0
    cv::Mat display, heat;
    //cv::namedWindow("Display window", WINDOW_AUTOSIZE);
    score_maps[0].convertTo(display, CV_8UC1, 200, 0);
    cv::applyColorMap(display, heat, COLORMAP_JET);
    //heat = heat.t();
    cv::imshow("before imagesc", heat);
#endif


    cv::Mat croppedImage;
    //std::cout<<"center.x = "<<center.x<<", center.y = "<<center.y<<std::endl;
    //std::cout<<"center.x - w/2 = "<<center.x - w/2<<std::endl;
    //std::cout<<"center.y - h/2 = "<<center.y - h/2<<std::endl;

    /*
   zx,
   X-Y coordinate system in OpenCV
   Low           High
   ---------------> x
   |
   |
   |
   |
   |
   V High
   y

   */

    /*    for (int i = 0; i < pad.size(); i++) {
        std::cout<<"pad["<<i<<"] = "<<pad[i]<<std::endl;
    }
*/
    // zx, cv::Rect: default starting points of x and y is the coordinate of the top-left corner.
    if (pad[0] > 0) {
        // crop up
        cv::Rect cropROI((center.x - w/2), (center.y - h/2 + pad[0]), w, (h - pad[0]));
        for (int i = 0; i < score_maps.size(); i++) {
            out_score_maps.push_back((score_maps[i](cropROI)).clone());
        }
    } else {
        // pad up
        cv::Scalar value(0, 0, 0);
        cv::Mat temp;
        for (int i = 0; i < (score_maps.size() - 1); i++) {
            cv::copyMakeBorder(score_maps[i], temp, -pad[0], 0, 0, 0, cv::BORDER_CONSTANT, value);
            out_score_maps.push_back(temp.clone());
        }
        cv::copyMakeBorder(score_maps[score_maps.size() - 1], temp, -pad[0], 0, 0, 0, cv::BORDER_CONSTANT, cv::Scalar(1, 1, 1));
        out_score_maps.push_back(temp.clone());
    }
    //    std::cout<<"crop.rows = "<<score_maps[0].rows<<", crop.cols = "<<score_maps[0].cols<<std::endl;

    if (pad[1] > 0) {
        // crop left
        int h = out_score_maps[0].rows;
        int w = out_score_maps[0].cols;
        cv::Point2f center(w/2, h/2);

        cv::Rect cropROI((center.x - w/2 + (pad[1])), (center.y - h/2), (w - pad[1]), h);
        for (int i = 0; i < out_score_maps.size(); i++) {
            //out_score_maps.push_back((out_score_maps[i](cropROI)).clone());
            out_score_maps[i] = out_score_maps[i](cropROI);
        }
    } else {
        // pad left
        Scalar value(0, 0, 0);
        for (int i = 0; i < (out_score_maps.size() - 1); i++) {
            cv::copyMakeBorder(out_score_maps[i], out_score_maps[i], 0, 0, -pad[1], 0, cv::BORDER_CONSTANT, value);
        }
        cv::copyMakeBorder(out_score_maps[out_score_maps.size() - 1], out_score_maps[out_score_maps.size() - 1], 0, 0, -pad[1], 0, cv::BORDER_CONSTANT, cv::Scalar(1, 1, 1));
    }


    if (pad[2] > 0) {
        // crop down
        int h = out_score_maps[0].rows;
        int w = out_score_maps[1].cols;
        cv::Point2f center(w/2, h/2);

        cv::Rect cropROI((center.x - w/2), (center.y - h/2), w, (h - pad[2]));
        for (int i = 0; i < out_score_maps.size(); i++) {
            out_score_maps[i] = out_score_maps[i](cropROI);
        }
    } else {
        // pad down
        Scalar value(0, 0, 0);
        for (int i = 0; i < (out_score_maps.size() - 1); i++) {
            cv::copyMakeBorder(out_score_maps[i], out_score_maps[i], 0, -pad[2], 0, 0, cv::BORDER_CONSTANT, value);
        }
        cv::copyMakeBorder(out_score_maps[out_score_maps.size() - 1], out_score_maps[out_score_maps.size() - 1], 0, -pad[2], 0, 0, cv::BORDER_CONSTANT, cv::Scalar(1, 1, 1));
    }


    if (pad[3] > 0) {
        // crop right

    } else {
        // pad right
        cv::Scalar value(0, 0, 0);
        for (int i = 0; i < (out_score_maps.size() - 1); i++) {
            cv::copyMakeBorder(out_score_maps[i], out_score_maps[i], 0, 0, 0, -pad[3], cv::BORDER_CONSTANT, value);
        }
        cv::copyMakeBorder(out_score_maps[out_score_maps.size() - 1], out_score_maps[out_score_maps.size() - 1], 0, 0, 0, -pad[3], cv::BORDER_CONSTANT, cv::Scalar(1, 1, 1));
    }

#if 0
    out_score_maps[0].convertTo(display, CV_8UC1, 200, 0);
    cv::applyColorMap(display, heat, COLORMAP_JET);
    //heat = heat.t();
    cv::imshow("after imagesc", heat);
    cv::waitKey(0);
#endif

}




template<typename Dtype>
void padAround(const cv::Mat &imageToTest,
               const int &boxsize,
               const cv::Point2f &center,
               cv::Mat &imageToTest_scaled,
               std::vector<int> &pad) {

    //std::cout<<"enter padAround"<<std::endl;
    //std::cout<<"center.x = "<<center.x<<", center.y = "<<center.y<<std::endl;

    int im_h = imageToTest.rows;
    int im_w = imageToTest.cols;

    // [] operator is fater than push_back
    pad[0] = (boxsize/2 - center.y);           // up
    pad[1] = (boxsize/2 - center.x);           // left
    pad[2] = (boxsize/2 - (im_h - center.y));  // down
    pad[3] = (boxsize/2 - (im_w - center.x));  // right

    //std::cout<<"pad[0] = "<<pad[0]<<", pad[1] = "<<pad[1]<<", pad[2] = "<<pad[2]<<", pad[3] = "<<pad[3]<<std::endl;

    //std::cout<<"imageToTest, 0 = "<<cv::sum(imageToTest).val[0]<<std::endl;
    //std::cout<<"imageToTest, 1 = "<<cv::sum(imageToTest).val[1]<<std::endl;
    //std::cout<<"imageToTest, 2 = "<<cv::sum(imageToTest).val[2]<<std::endl;

    // cv::copyMakeBorder(src, dst, top, bottom, left, right, borderType, value);
    cv::Mat temp;
    // filling value
    Scalar value(128, 128, 128);
    if (pad[0] > 0) {
        // filling up
        cv::copyMakeBorder(imageToTest, temp, pad[0], 0, 0, 0, cv::BORDER_CONSTANT, value);
    } else {
        cv::copyMakeBorder(imageToTest, temp, 0, 0, 0, 0, cv::BORDER_CONSTANT, value);
    }

    if (pad[1] > 0) {
        // filling left
        cv::copyMakeBorder(temp, temp, 0, 0, pad[1], 0, cv::BORDER_CONSTANT, value);
    } else {
        cv::copyMakeBorder(temp, temp, 0, 0, 0, 0, cv::BORDER_CONSTANT, value);
    }

    if (pad[2] > 0) {
        // filling down
        cv::copyMakeBorder(temp, temp, 0, pad[2], 0, 0, cv::BORDER_CONSTANT, value);
    } else {
        cv::copyMakeBorder(temp, temp, 0, 0, 0, 0, cv::BORDER_CONSTANT, value);
    }

    if (pad[3] > 0) {
        // filling right
        cv::copyMakeBorder(temp, temp, 0, 0, 0, pad[3], cv::BORDER_CONSTANT, value);
    } else {
        cv::copyMakeBorder(temp, temp, 0, 0, 0, 0, cv::BORDER_CONSTANT, value);
    }

    cv::Point2f center_padded;
    center_padded = center + cv::Point2f(std::max(0, pad[1]), std::max(0, pad[0]));
    //std::cout<<"cv::Point2f(std::max(0, pad[1]), std::max(0, pad[0])) = "<<cv::Point2f(std::max(0, pad[1]), std::max(0, pad[0]))<<std::endl;
    //std::cout<<"center_padded = "<<center_padded<<", center = "<<center<<std::endl;

    // setup a croping region
    cv::Rect cropROI((center_padded.x - boxsize/2), (center_padded.y - boxsize/2), boxsize, boxsize);

    //std::cout<<"cropROI = "<<cropROI<<std::endl;
    cv::Mat croppedImage = temp(cropROI);

    imageToTest_scaled = croppedImage.clone();

    //std::cout<<"croppedImage.rows = "<<croppedImage.rows<<",croppedImage.cols = "<<croppedImage.cols<<", croppedImage.channels = "<<croppedImage.channels()<<std::endl;


    //std::cout<<"temp.rows = "<<temp.rows<<", temp.cols = "<<temp.cols<<", temp.channels = "<<temp.channels()<<std::endl;
    //for (int r = 0; r < 1; r++)
    //  for (int index = 0; index < temp.cols; index++)
    //    std::cout<<(int)temp.at<cv::Vec3b>(r, index)[0]<<" ";
    //  std::cout<<" "<<std::endl;

    //cv::namedWindow("Display window", WINDOW_AUTOSIZE);
    //cv::imshow("Display windows", croppedImage);
    //waitKey(0);


    return;

}



int main(int argc, char** argv)
{
//std::cout << cv::getBuildInformation() << std::endl; return 0;

#ifdef SENDTOCLIENT
    ToTK1 totk1;
    totk1.Start("/home/nvidia/H.yml");

    ToST tst;
    tst.StartUpdate();
#endif

    // init camera
    VideoCapture cap(1); // open the default camera
    if(!cap.isOpened()) {  // check if we succeeded
        std::cout<<"cap can not be open"<<std::endl;
        return -1;
    }

    
/*    Mat img;
	cap.read(img);
	std::cout << img.size() << std::endl;
	
    return 0;
*/    typedef float Dtype;
    // init neural network
    ::google::InitGoogleLogging(argv[0]);
    const int num_required_args = 3;
    if (argc < num_required_args) {
        LOG(ERROR)<<
                     "This program takes in a trained network and an input data layer, and then"
                     "perform the test."
                     "Usage: test_capacitor_model pretrained_net_param"
                     "proto_file"
                     "[CPU/GPU] [DEVICE_ID=0]\n";

        return 1;
    }

    std::string joint_dist_file = "../model/joint_dist_20160601.mat";

    //-------------------------------------------------------------------------
    //Compute distance between each pair of keypoints [for postprocessing]
    //-------------------------------------------------------------------------

/*    arma::mat mean, var;
    bool status = mean.load(joint_dist_file);

    if (status == true) {
        std::cout<<"success"<<std::endl;
    } else {
        std::cout<<"failed"<<std::endl;
    }
*/
    float means[][6] = {
        0, 14.2964, 74.0247, 76.9347, 24.8896, 11.4096,
        14.2964, 0, 65.5008, 74.8344, 35.6525, 24.4767,
        74.0247, 65.5008, 0, 36.2563, 74.5882, 76.7578,
        76.9347, 74.8344, 36.2563, 0, 65.4710, 74.2755,
        24.8896, 35.6525, 74.5882, 65.4710, 0, 14.6950,
        11.4096, 24.4767, 76.7578, 74.2755, 14.6950, 0
    };

#if 0
    // vars == 4.46*std
    float vars[][6] {
        0, 10.6754, 8.9460, 9.3729, 12.8544, 18.3736,
        10.6754, 0, 7.5255, 9.9703, 10.9767, 12.1300,
        8.9460, 7.5255, 0, 10.8583, 9.7945, 8.7154,
        9.3729, 9.9703, 10.8583, 0, 8.1166, 8.4773,
        12.8544, 10.9767, 9.7945, 8.1166, 0, 10.7708,
        18.3736, 12.1300, 8.7154, 8.4773, 10.7708, 0
    };
#endif

#if 0
    // vars == 6*std
    float vars[][6] = {
        0, 14.3615, 12.0350, 12.6092, 17.2930, 24.7178,
        14.3615, 0, 10.1241, 13.4130, 14.7669, 16.3184,
        12.0350, 10.1241, 0, 14.6075, 13.1765, 11.7248,
        12.6092, 13.4130, 14.6075, 0, 10.9192, 11.4045,
        17.2930, 14.7669, 13.1765, 10.9192, 0, 14.4898,
        24.7178, 16.3184, 11.7248, 11.4045, 14.4898, 0
    };
#endif

#if 1
    // vars == 8*std
    float vars[][6] = {
        0, 19.14867, 16.0467, 16.8123, 23.0573, 32.9571,
        19.1487, 0, 13.4988, 17.884, 19.6892, 21.7579,
        16.0467, 13.4988, 0, 19.4767, 17.5687, 15.6331,
        16.8123, 17.884, 19.4767, 0, 14.5589, 15.206,
        23.0573, 19.6892, 17.5687, 14.5589, 0, 19.3197,
        32.9571, 21.7579, 15.6331, 15.206, 19.3197, 0
    };
#endif

    // -------------------------------------------------------------------------
    // initialize network
    // -------------------------------------------------------------------------
    int arg_pos = num_required_args;
    if (argc > arg_pos && strcmp(argv[arg_pos], "GPU") == 0) {
        LOG(INFO)<<"Using GPU";
        uint device_id = 0;
        if (argc > arg_pos + 1) {
            device_id = atoi(argv[arg_pos + 1]);
            CHECK_GE(device_id, 0);
        }
        LOG(INFO)<<"Using Device_id = "<<device_id;

        caffe::Caffe::SetDevice(device_id);
        caffe::Caffe::set_mode(caffe::Caffe::GPU);
    } else {
        LOG(INFO)<<"Using CPU";
        caffe::Caffe::set_mode(caffe::Caffe::CPU);
    }


    arg_pos = 0; // the name of the exxcutable
    std::string pretrained_binary_proto(argv[++arg_pos]);
    std::cout<<"pretrained_binary_proto = "<<pretrained_binary_proto<<std::endl;
    std::string net_proto(argv[++arg_pos]);
    std::cout<<"net_proto = "<<net_proto<<std::endl;
    boost::shared_ptr<caffe::Net<Dtype> > test_capacitor_net(new caffe::Net<Dtype>(net_proto, caffe::TEST));

    test_capacitor_net->CopyTrainedLayersFrom(pretrained_binary_proto);
    boost::shared_ptr<caffe::MemoryDataLayer<Dtype> > md_layer = boost::dynamic_pointer_cast<caffe::MemoryDataLayer<Dtype> >(test_capacitor_net->layers()[0]);
    CHECK (md_layer) << "The first layer is not a MemoryDataLayer!";

    // network has been initialized successfully

    // -------------------------------------------------------------------------
    // inference
    // -------------------------------------------------------------------------
    float target_dist = 160 * 368 / 1200;   // matching the training image scale
    int boxsize = 368;                      // input crop
    int scale_provided = 74;
    std::vector<int> scale_search;          // 0.7, 0.3, 1.3, multi-scale testing
    scale_search.push_back(1);

    //int np = 6;                             // number of keypoints

    std::vector<Dtype> forward_stat, pp_stat;



    // get each frame from camera
    //    Mat frame;
    //    cap.read(frame);
    //    std::cout<<"size(frame) = "<<frame.size()<<std::endl;

    //    double count = cap.get(CV_CAP_PROP_FPS);
    //    std::cout<<"rate = "<<count<<std::endl;

    Size size(1028, 720);
    int num_average_frames = 0;
    std::vector<cv::Mat> final_score_last_frame;




    int cnt=0;
    while(1)
    {
        clock_t t_fw_start = clock();

        num_average_frames++;

        Mat frame, orimage;
        cnt++;
        //cap >> frame; // get a new frame from camera
        //        cap.read(frame);
        //std::cout<<"size(frame) = "<<frame.size()<<std::endl;
        //cvtColor(frame, edges, CV_BGR2GRAY);
        //GaussianBlur(edges, edges, Size(7,7), 1.5, 1.5);
        //Canny(edges, edges, 0, 30, 3);
        //imshow("edges", edges);

        while (1) {
            auto t1 = std::chrono::high_resolution_clock::now();
            cap.grab();

            auto t2 = std::chrono::high_resolution_clock::now();
            auto fp_ms = std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1);
            if (fp_ms.count() * 30 > 0.5) {
                break;
            }
        }
        cap.retrieve(frame);

        resize(frame, orimage, size);

        if (orimage.rows != 720) {
            cv::resize(orimage, orimage, size, 0, 0, INTER_CUBIC);
            //            std::cout<<"new size = "<<size<<std::endl;
        }

        cv::Point2f center((orimage.cols / 2), (orimage.rows / 2));


        float scale0 = target_dist / scale_provided;
        //        std::cout<<"target_dist = "<<target_dist<<", scale_provided = "<<scale_provided<<", scale0 = "<<scale0<<std::endl;

        std::vector<cv::Mat> imageToTestPadded(1);
        std::vector<std::vector<int> > pad(1, vector<int>(4));
        std::vector<std::vector<cv::Mat>> scores(scale_search.size());
        std::vector<std::vector<cv::Mat>> scores_without_padding(scale_search.size());
        //        scores_without_padding.clear();
        clock_t t_fw_end_1 = clock();
        Dtype duration_1 =(t_fw_end_1 - t_fw_start)*1000/CLOCKS_PER_SEC;
        //        std::cout<<"duration_1 = "<<duration_1<<std::endl;
        // processing multiple scales on each image

        for (int index = 0; index < scale_search.size(); index++) {
            // prepare data
            float scale = scale0 * scale_search[index];
            cv::Size size(ceil(orimage.cols*scale), ceil(orimage.rows*scale));
            cv::Mat imageToTest(size, CV_8UC3);

            cv::resize(orimage, imageToTest, size, 0, 0, INTER_CUBIC);

            cv::Point2f center_s(round(center.x * scale), round(center.y * scale));
            //std::cout<<"center_s = "<<center_s<<std::endl;

            padAround<float>(imageToTest, boxsize, center_s, imageToTestPadded[index], pad[index]);

            cv::Mat imagePreProcessed;
            preprocess<float>(imageToTestPadded[index], 0.5, imagePreProcessed);

            std::vector<caffe::Blob<Dtype>*> cap_net_input_blobs;
            cap_net_input_blobs = test_capacitor_net->input_blobs();
            //            std::cout<<"cap_net_input_blobs.size() = "<<cap_net_input_blobs.size()<<std::endl;
            //for (index = 0; index < blobs_name_zx.size(); index++) {
            //  std::cout<<"blobs_name = "<<blobs_name_zx[index]<<std::endl;
            //}

            // zx, because the memorydata layer existing implementation only supports one scale for each image, in order to handle each image with multiple scales,
            // we preprocess each image including scale, mean substraction, and crop, outside the memorydata layer.
            // the memorydata layer only need to receive each image and forward propagate each image to infer it.
            std::vector<cv::Mat> input_images(1, imagePreProcessed);

            //            std::cout<<"size = "<<input_images.size()<<std::endl;
            //            std::cout<<"input_images.size = "<<input_images[0].size()<<std::endl;
            std::vector<int> dummy_vector (1, 0);
            md_layer->AddMatVector(input_images, dummy_vector);
            clock_t t_fw_end_2 = clock();
            Dtype duration_2 =(t_fw_end_2 - t_fw_start)*1000/CLOCKS_PER_SEC;
            //            std::cout<<"duration_2 = "<<duration_2<<std::endl;

            // go through caffe
            test_capacitor_net->ForwardFromTo(0, test_capacitor_net->layers().size() - 1);

            clock_t t_fw_end_3 = clock();
            Dtype duration_3 =(t_fw_end_3 - t_fw_start)*1000/CLOCKS_PER_SEC;
            //            std::cout<<"duration_3 = "<<duration_3<<std::endl;


            //const boost::shared_ptr<caffe::Blob<Dtype> > data_blob = test_capacitor_net->blob_by_name(std::string("data"));
            //const boost::shared_ptr<caffe::Blob<Dtype> > pool1_blob = test_capacitor_net->blob_by_name(std::string("conv1_stage1"));
            const boost::shared_ptr<caffe::Blob<Dtype> > feature_blob = test_capacitor_net->blob_by_name(std::string("Fconv7_stage1"));

            // obtain score maps
            const Dtype* feature_blob_data = feature_blob->cpu_data();
            const std::vector<int> feature_blob_shape = feature_blob->shape();
            int score_map_size = feature_blob_shape[2] * feature_blob_shape[3];
            Dtype score_channel[score_map_size];


            for (int c = 0; c < feature_blob_shape[1]; c++) {
                for (int i = 0; i < score_map_size; i++) {
                    score_channel[i] = (feature_blob_data + feature_blob->offset(0, c))[i];
                }
                cv::Mat score_mat = cv::Mat(feature_blob_shape[2], feature_blob_shape[3], CV_32FC1, score_channel);
                scores[index].push_back(score_mat.clone());
            }

            int pool_time = imagePreProcessed.rows / scores[index][0].rows;
            //std::cout<<"pool_time = "<<pool_time<<std::endl;
            //std::cout<<"imagePreProcessed.rows = "<<imagePreProcessed.rows<<std::endl;
            //std::cout<<"scores[index][0].rows = "<<scores[index][0].rows<<std::endl;

            cv::Size input_size((scores[index][0].cols*pool_time), (scores[index][0].rows*pool_time));
            for (int i = 0; i < scores[index].size(); i++) {
                cv::resize(scores[index][i], scores[index][i], input_size, 0, 0, INTER_CUBIC);
            }

            //std::cout<<"(scores[index][0].cols*pool_time) = "<<(scores[index][0].cols)<<std::endl;
            //std::cout<<"(scores[index][0].rows*pool_time) = "<<(scores[index][0].rows)<<std::endl;
            resizeIntoScaledImg<float>(scores[index], pad[index], scores_without_padding[index]);

            cv::Size scores_w_p_size(orimage.cols, orimage.rows);
            //cv::Mat temp_score = cv::Mat(orimage.rows, orimage.cols, CV_32FC1);
            int length = scores_without_padding[index].size();
            for (int i = 0; i < length; i++) {
                cv::resize(scores_without_padding[index][i], scores_without_padding[index][i], scores_w_p_size, 0, 0, INTER_CUBIC);
            }
            //clock_t t_start = clock();
            //cv::resize(scores_without_padding[index][0], temp_score, scores_w_p_size);
            //clock_t t_end = clock();
            //std::cout<<(t_end - t_start)*1000/CLOCKS_PER_SEC<<" ms."<<std::endl;

            //cv::resize(scores_without_padding[index][i], temp_score, scores_w_p_size, 0, 0, INTER_CUBIC);
            //cv::resize(scores_without_padding[index][i], temp_score, scores_w_p_size, 0, 0, INTER_CUBIC);
            //cv::resize(scores_without_padding[index][i], temp_score, scores_w_p_size, 0, 0, INTER_CUBIC);


            //            std::cout<<"scores_without_padding.rows = "<<scores_without_padding[index][0].rows<<", scores_without_padding.cols = "<<scores_without_padding[index][0].cols<<std::endl;
#if 0
            cv::Mat display, heat;
            scores_without_padding[index][0].convertTo(display, CV_8UC1, 200, 0);
            cv::applyColorMap(display, heat, COLORMAP_JET);
            //heat = heat.t();
            cv::imshow("imagesc", heat);
            cv::waitKey(0);
#endif
        }


        // summing up scores (fusing multiscale scores)
        std::vector<cv::Mat> final_score;
        final_score.clear();
        cv::Mat tempp = cv::Mat(scores_without_padding[0][0].rows, scores_without_padding[0][0].cols, CV_32FC1, Dtype(0.0));
        for (int c = 0; c < scores_without_padding[0].size(); c++) {
            cv::Mat temp = cv::Mat::zeros(scores_without_padding[0][0].rows, scores_without_padding[0][0].cols, CV_32FC1);
            for (int i = 0; i < scores_without_padding.size(); i++) {
                cv::add(temp, scores_without_padding[i][c], temp);
            }
            final_score.push_back(temp);
        }

        //#if 0
        if (num_average_frames == 1) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame.push_back(final_score[index]);
            }
            final_score_last_frame.clear();
            num_average_frames = 0;
            //            continue;

        }

#if 0
        else if (num_average_frames == 2) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }
        else if (num_average_frames == 3) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score[index] = final_score[index] + final_score_last_frame[index];
                final_score[index] = final_score[index] / 2.0f;
            }

            final_score_last_frame.clear();
            num_average_frames = 0;
        }
        //#endif

        //#if 0
        else if (num_average_frames == 3) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        } else if (num_average_frames == 4) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }

        else if (num_average_frames == 5) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }

        else if (num_average_frames == 6) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }

        else if (num_average_frames == 7) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }

        else if (num_average_frames == 8) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }

        else if (num_average_frames == 9) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }

        else if (num_average_frames == 10) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }

        else if (num_average_frames == 11) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }


        else if (num_average_frames == 12) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }



        else if (num_average_frames == 13) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }



        else if (num_average_frames == 14) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }


        else if (num_average_frames == 15) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }


        else if (num_average_frames == 16) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }


        else if (num_average_frames == 17) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }


        else if (num_average_frames == 18) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }

        else if (num_average_frames == 19) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }

        else if (num_average_frames == 20) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }


        else if (num_average_frames == 21) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }



        else if (num_average_frames == 22) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }



        else if (num_average_frames == 23) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }


        else if (num_average_frames == 24) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }


        else if (num_average_frames == 25) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }


        else if (num_average_frames == 26) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }


        else if (num_average_frames == 27) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score_last_frame[index] = final_score[index] + final_score_last_frame[index];
                final_score_last_frame[index] = final_score_last_frame[index] / 2.0f;
            }

            continue;
        }
        else if (num_average_frames == 28) {
            for (int index = 0; index < final_score.size(); index++) {
                final_score[index] = final_score[index] + final_score_last_frame[index];
                final_score[index] = final_score[index] / 2.0f;
            }

            final_score_last_frame.clear();
            num_average_frames = 0;
        }
#endif

        clock_t t_fw_end = clock();
        Dtype duration =(t_fw_end - t_fw_start)*1000/CLOCKS_PER_SEC;
        //        std::cout<<"forward: "<<duration<<" ms."<<std::endl;
        forward_stat.push_back(duration);
        // -------------------------------------------------------------------------
        // post processing
        // -------------------------------------------------------------------------
        clock_t t_pp_start = clock();

        /*
                1  6
              __|__|__
            2 |      | 5
              |      |
              |      |
              |      |
            3 |______| 4

            capacitor

        */

        // Step 1: find local max on each score map
        std::vector<std::vector<cv::Rect> > bboxes((final_score.size() - 2));
        thresh_callback(final_score[0],5,bboxes[0]);
        for (int i = 1; i < (final_score.size() - 3); i++) {
            thresh_callback(final_score[i], 20, bboxes[i]);
        }

        thresh_callback(final_score[5],5,bboxes[5]);

        std::vector<std::vector<cv::Point> > peaks((final_score.size() - 2));
#if 1
        for (int i = 0; i < (final_score.size() - 2); i++) {
            for (int j = 0; j < bboxes[i].size(); j++) {
                cv::Mat mask = cv::Mat::zeros(final_score[0].rows, final_score[1].cols, CV_8UC1);
                mask(bboxes[i][j]) = 1;

                double min, max;
                cv::Point min_loc, max_loc;
                cv::minMaxLoc(final_score[i], &min, &max, &min_loc, &max_loc, mask);

                //x = max_loc.x;
                //y = max_loc.y;

                peaks[i].push_back(max_loc);
            }
        }
#endif

        std::vector<std::pair<cv::Point, cv::Point> > edge_34;
        for (int i = 0; i < peaks[2].size(); i++) {
            for (int j = 0; j < peaks[3].size(); j++) {
                cv::Point diff;
                diff = peaks[2][i] - peaks[3][j];
                //std::cout<<"peaks[2]["<<i<<"] = "<<peaks[2][i]<<", x = "<<peaks[2][i].x<<", y = "<<peaks[2][i].y<<std::endl;
                //std::cout<<"peaks[3]["<<j<<"] = "<<peaks[3][j]<<std::endl;
                //std::cout<<"diff = "<<diff<<std::endl;
                Dtype distance = cv::norm(diff);
                //std::cout<<"distance = "<<distance<<std::endl;
                //std::cout<<"means[2][3] = "<<means[2][3]<<", vars[2][3] = "<<vars[2][3]<<std::endl;
                if ((std::abs(distance - means[2][3])) < vars[2][3]) {
                    edge_34.push_back(std::make_pair(peaks[2][i], peaks[3][j]));
                }
            }
        }

        std::vector<int> multiplier;
        multiplier.push_back(1);
        multiplier.push_back(-1);
        std::vector<std::pair<std::vector<cv::Point>, Dtype> > image_obj;
        image_obj.clear();

        for (int index = 0; index < edge_34.size(); index++) {
            std::pair<std::vector<cv::Point>, Dtype> curobj;
            for (int j = 0; j < multiplier.size(); j++) {
                // Step 3.1 find edge with keypoint [3, 2]
                arma::mat A = arma::randu<arma::mat>(1,2);
                std::vector<Dtype> basis;
                A.row(0).col(0) = edge_34[index].first.x - edge_34[index].second.x;
                A.row(0).col(1) = edge_34[index].first.y - edge_34[index].second.y;
                //std::cout<<"A.row(0).col(0) = "<<A.row(0).col(0)<<", A.row(0).col(1) = "<<A.row(0).col(1)<<std::endl;
                arma::mat B = arma::null(A);

                //std::cout<<"B.n_rows = "<<B.n_rows<<", B.n_cols = "<<B.n_cols<<std::endl;
                // B: 2rows and 1col
                for (int i = 0; i < B.n_rows; i++) {
                    //std::cout<<"B["<<i<<"] = "<<B[i]<<std::endl;
                    basis.push_back(B[i]);
                }

                std::vector<Dtype> temp_p2(2, 0);
                cv::Point point_2;
                for (int i = 0; i < basis.size(); i++) {
                    temp_p2[i] = basis[i] * multiplier[j] * means[2][1];
                    //std::cout<<"after, basis["<<i<<"] = "<<basis[i]<<std::endl;
                }

                point_2 = cv::Point(edge_34[index].first.x + temp_p2[0], edge_34[index].first.y + temp_p2[1]);
                std::vector<cv::Point> candidate_p2;
                Dtype min_distance = 1000000;
                //std::cout<<"eee"<<std::endl;
                for (int i = 0; i < peaks[1].size(); i++) {
                    cv::Point diff;
                    diff = point_2 - peaks[1][i];
                    //std::cout<<"2342"<<std::endl;
                    Dtype distance = cv::norm(diff);
                    if (distance < vars[2][1]) {
                        if (candidate_p2.size() == 0) {
                            candidate_p2.push_back(peaks[1][i]);
                            min_distance = distance;
                        } else {
                            if (distance < min_distance) {
                                //std::cout<<"1111"<<std::endl;
                                candidate_p2[0].x = peaks[1][i].x;
                                candidate_p2[0].y = peaks[1][i].y;
                                //std::cout<<"2222"<<std::endl;
                                min_distance = distance;
                            }
                        }
                    }
                }

                if (candidate_p2.size() == 0) {
                    continue;
                }

                //std::cout<<"werwer"<<std::endl;


                // Step 3.2 find edge with keypoint [4,5]
                std::vector<Dtype> temp_p5(2, 0);
                cv::Point point_5;
                for (int i = 0; i < basis.size(); i++) {
                    temp_p5[i] = basis[i] * multiplier[j] * means[3][4];
                }

                point_5 = cv::Point(edge_34[index].second.x + temp_p5[0], edge_34[index].second.y + temp_p5[1]);
                std::vector<cv::Point> candidate_p5;
                Dtype min_distance_45 = 1000000;
                //std::cout<<"4564"<<std::endl;
                for (int i = 0; i < peaks[4].size(); i++) {
                    cv::Point diff;
                    diff = point_5 - peaks[4][i];
                    Dtype distance = cv::norm(diff);
                    //std::cout<<"67567"<<std::endl;
                    if (distance < vars[3][4]) {
                        if (candidate_p5.size() == 0) {
                            candidate_p5.push_back(peaks[4][i]);
                            min_distance_45 = distance;
                            //std::cout<<"78678"<<std::endl;
                        } else {
                            if (distance < min_distance_45) {
                                //std::cout<<"xcvxcv"<<std::endl;
                                candidate_p5[0].x = peaks[4][i].x;
                                candidate_p5[0].y = peaks[4][i].y;
                                min_distance = min_distance_45;
                                //std::cout<<"345"<<std::endl;
                            }
                        }
                    }
                }

                if (candidate_p5.size() == 0) {
                    continue;
                }

                if (candidate_p2.size() == 1 && candidate_p5.size() == 1) {
                    cv::Point diff;
                    diff = candidate_p2[0] - candidate_p5[0];
                    Dtype distance = cv::norm(diff);
                    if ((std::abs(distance - means[1][4])) >= vars[1][4]) {
                        continue;
                    }
                } else {
                    LOG(ERROR)<<"size != one\n";
                }

                Dtype slope54 = (candidate_p5[0].y - edge_34[index].second.y) / (Dtype)(candidate_p5[0].x - edge_34[index].second.x);
                Dtype slope23 = (candidate_p2[0].y - edge_34[index].first.y) / (Dtype)(candidate_p2[0].x - edge_34[index].first.x);
                Dtype slope34 = (edge_34[index].first.y - edge_34[index].second.y) / (Dtype)(edge_34[index].first.x - edge_34[index].second.x);
                Dtype slope25 = (candidate_p2[0].y - candidate_p5[0].y) / (Dtype)(candidate_p2[0].x - candidate_p5[0].x);

                //std::cout<<"slop54 = "<<slope54<<", slope23 = "<<slope23<<", slope34 = "<<slope34<<", slope25 = "<<slope25<<std::endl;

                //if (std::abs(slope25 - slope34) > 0.5 || std::abs(slope54 - slope23) > 0.5) {
                //    continue;
                //}

                // Step 3.4: find the constraint of pin 1
                cv::Point mid_point_25 = (candidate_p2[0] + candidate_p5[0]) * (0.5);
                std::vector<Dtype> temp_p1(2, 0);
                cv::Point point_1;
                for (int i = 0; i < basis.size(); i++) {
                    temp_p1[i] = basis[i] * multiplier[j] * means[1][0];
                }
                point_1 = cv::Point(mid_point_25.x + temp_p1[0], mid_point_25.y + temp_p1[1]);
                std::vector<cv::Point> candidate_p1;
                Dtype min_distance_12 = 1000000;
                for (int i = 0; i < peaks[0].size(); i++) {
                    cv::Point diff;
                    diff = point_1 - peaks[0][i];
                    Dtype distance = cv::norm(diff);

                    if (distance < vars[1][0]) {
                        if (candidate_p1.size() == 0) {
                            candidate_p1.push_back(peaks[0][i]);
                            min_distance_12 = distance;
                            //std::cout<<"78678"<<std::endl;
                        } else {
                            if (distance < min_distance_12) {
                                //std::cout<<"xcvxcv"<<std::endl;
                                candidate_p1[0].x = peaks[0][i].x;
                                candidate_p1[0].y = peaks[0][i].y;
                                min_distance = min_distance_12;
                                //std::cout<<"345"<<std::endl;
                            }
                        }
                    }
                }
                if (candidate_p1.size() == 0) {
                    continue;
                }


                // Step 3.5: find the constraint of pin 6
                //cv::Point mid_point_25 = (candidate_p2[0] + candidate_p5[0]) * (0.5);
                std::vector<Dtype> temp_p6(2, 0);
                cv::Point point_6;
                for (int i = 0; i < basis.size(); i++) {
                    temp_p6[i] = basis[i] * multiplier[j] * means[4][5];
                }
                point_6 = cv::Point(mid_point_25.x + temp_p6[0], mid_point_25.y + temp_p6[1]);

                std::vector<cv::Point> candidate_p6;
                Dtype min_distance_56 = 1000000;
                for (int i = 0; i < peaks[5].size(); i++) {
                    cv::Point diff;
                    diff = point_6 - peaks[5][i];
                    Dtype distance = cv::norm(diff);

                    if (distance < vars[4][5]) {
                        if (candidate_p6.size() == 0) {
                            candidate_p6.push_back(peaks[5][i]);
                            min_distance_56 = distance;
                            //std::cout<<"78678"<<std::endl;
                        } else {
                            if (distance < min_distance_56) {
                                //std::cout<<"xcvxcv"<<std::endl;
                                candidate_p6[0].x = peaks[5][i].x;
                                candidate_p6[0].y = peaks[5][i].y;
                                min_distance = min_distance_56;
                                //std::cout<<"345"<<std::endl;
                            }
                        }
                    }
                }
                if (candidate_p6.size() == 0) {
                    continue;
                }

                std::vector<cv::Point> joints;
                joints.push_back(candidate_p1[0]);
                joints.push_back(candidate_p2[0]);
                joints.push_back(edge_34[index].first);
                joints.push_back(edge_34[index].second);
                joints.push_back(candidate_p5[0]);
                joints.push_back(candidate_p6[0]);

                Dtype average_score;
                std::vector<Dtype> joint_score;
                object_score<float>(joints, final_score, joint_score, average_score);

                std::vector<cv::Point> final_joints(6);
                std::vector<Dtype> final_joint_scores(6);
                refine_joints(joints, joint_score, final_joints, final_joint_scores);

                if (curobj.first.size() == 0) {
                    curobj = std::make_pair(final_joints, average_score);
                } else {
                    if (average_score > curobj.second) {
                        for (int i = 0; i < final_joints.size(); i++) {
                            curobj.first[i] = final_joints[i];
                        }
                        curobj.second = average_score;
                    }
                }
            }

            if (curobj.first.size() != 0) {
                image_obj.push_back(curobj);
            }
        }

        clock_t t_pp_end = clock();

        Dtype pp_duration = (t_pp_end - t_pp_start)*1000/CLOCKS_PER_SEC;

        // -------------------------------------------------------------------------
        // show results
        // -------------------------------------------------------------------------
        std::cout<<"image_obj.size() = "<<image_obj.size()<<std::endl;

#ifdef SAVETOTXT
        //Save Result to TEXT File
        //Create txt file to store detected cap location (Center and Orientation)
        std::ofstream location_output;
        location_output.open("location.txt");

        double pt1_x_scaled, pt1_y_scaled,pt2_x_scaled, pt2_y_scaled,pt3_x_scaled, pt3_y_scaled,pt4_x_scaled, pt4_y_scaled;
#endif

        float scaleX = 640.0/size.width; //640->640
        float scaleY = 480.0/size.height; //480->480



        for (int i = 0; i < image_obj.size(); i++) {
            if (image_obj[i].first[1].x !=0){
                cv::line(orimage, image_obj[i].first[1], image_obj[i].first[2], cv::Scalar(225, 0, 0), 2);
                cv::line(orimage, image_obj[i].first[2], image_obj[i].first[3], cv::Scalar(0, 255, 0), 2);
                cv::line(orimage, image_obj[i].first[3], image_obj[i].first[4], cv::Scalar(0, 0, 255), 2);
                cv::line(orimage, image_obj[i].first[4], image_obj[i].first[1], cv::Scalar(0, 255, 255), 2);

                int center_x = (image_obj[i].first[1].x+image_obj[i].first[2].x+image_obj[i].first[3].x+image_obj[i].first[4].x)/4;
                int center_y = (image_obj[i].first[1].y+image_obj[i].first[2].y+image_obj[i].first[3].y+image_obj[i].first[4].y)/4;

                cv::Point center = cv::Point(center_x, center_y);
                std::cout<<"center = "<<center<<std::endl;

                //Only Draw line if not both center_x and center_y are ZERO
                if (center_x != 0 && center_y != 0){
                    cv::line(orimage, center, image_obj[i].first[0], cv::Scalar(0, 255, 0), 2);
                    cv::line(orimage, center, image_obj[i].first[5], cv::Scalar(0, 255, 0), 2);
                }

                //cv::line(orimage, center, image_obj[i].first[0], cv::Scalar(0, 255, 0), 2);
                //cv::line(orimage, center, image_obj[i].first[5], cv::Scalar(0, 255, 0), 2);

                //cv::imshow("imagesc_init", orimage);
                //cv::waitKey(0);

                cv::Size show_size(ceil(orimage.cols), ceil(orimage.rows));
                cv::resize(orimage, orimage, show_size, 0, 0, INTER_CUBIC);
                //cv::imshow("imagesc", orimage);
                //cv::waitKey(0);





                //Center Scaling
                double center_x_scaled = center_x*scaleX;
                double center_y_scaled = center_y*scaleY;


#ifdef SENDTOCLIENT
                totk1.SetPixel(center_x_scaled, center_y_scaled);
#endif



#ifdef SAVETOTXT
                //Keypoint 1,2,3,4 scaled
                pt1_x_scaled = ceil(image_obj[i].first[1].x*scaleX);
                pt1_y_scaled = ceil(image_obj[i].first[1].y*scaleY);

                pt2_x_scaled = ceil(image_obj[i].first[2].x*scaleX);
                pt2_y_scaled = ceil(image_obj[i].first[2].y*scaleY);

                pt3_x_scaled = ceil(image_obj[i].first[3].x*scaleX);
                pt3_y_scaled = ceil(image_obj[i].first[3].y*scaleY);

                pt4_x_scaled = ceil(image_obj[i].first[4].x*scaleX);
                pt4_y_scaled = ceil(image_obj[i].first[4].y*scaleY);
#endif

            }
        }





#ifdef SAVETOTXT
        //Write Data (if the file is opened) San 20170806
        if (location_output.is_open()){

            location_output << pt1_x_scaled <<"\t"<<  pt1_y_scaled <<"\t"<< pt2_x_scaled <<"\t"<<  pt2_y_scaled <<"\t"<< pt3_x_scaled <<"\t"<<  pt3_y_scaled <<"\t"<< pt4_x_scaled <<"\t"<<  pt4_y_scaled <<"\n";
        }else{
            //Can't write. Show Error Message
            std::cout<<"Something went wrong with opening the file";
        }
        //End of writing file
        location_output.close();
#endif
        

        //cv::Size show_size(ceil(orimage.cols*0.8), ceil(orimage.rows*0.8));
        //cv::resize(orimage, orimage, show_size, 0, 0, INTER_CUBIC);

        //std::string file_name = "./" + std::to_string(img_index) + ".jpg";
        std::cout << cnt << std::endl;
        std::string file_name = "./" + std::to_string(cnt) + ".jpg";
        //        cv::imwrite(file_name, orimage);
        cv::imshow("detected", orimage);

        //cv::imshow("imagesc", orimage);
        //cv::waitKey(0);

        std::cout<<"post-processing: "<<pp_duration<<" ms."<<std::endl;
        pp_stat.push_back(pp_duration);

        imshow("video", frame);
        //sleep(0.9);
        if (waitKey(30) >= 0) {
            break;
        }




#ifdef SENDTOCLIENT
        totk1.WaitUntilMoved();

        bool isStop=tst.IsStop();
        if(!isStop)//if arm operating normally
        {
            while(!totk1.idle)//wait until arm move to idle pos
            {
                isStop=tst.IsStop();//update status
                if(isStop)
                {
                    break;
                }
                std::this_thread::yield();
            }
        }

        if(isStop)//if arm emergency stop
        {
            totk1.End();
            std::cout<<"STOP!WAIT TILL RESUME\n";
            while(tst.IsStop())
            {
                std::this_thread::yield();
            }
            std::cout<<"RESUME!\n";
            totk1.Start("/home/nvidia/H.yml");
        }


#endif

    } // while(1)

    Dtype t_forward = 0, t_pp = 0;
    for (int i = 0; i < forward_stat.size(); i++) {
        t_forward += forward_stat[i];
        t_pp += pp_stat[i];
    }
    std::cout<<"average fw: "<<t_forward/forward_stat.size()<<"ms, average pp: "<<t_pp/pp_stat.size()<<"ms."<<std::endl;

#ifdef SENDTOCLIENT
    totk1.End();
    tst.EndUpdate();
#endif

    // the camera will be deinitialized automatically in VideoCapture destructor
    return 0;
}

